﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace Spacer {
	public class Quadtree<T> {
		public delegate bool OverCapacity( Quadtree<T> tree, QuadtreeObject newest, List<QuadtreeObject> objects, int max );

		public struct QuadtreeObject {
			T m_Object;
			float m_fXPos, m_fYPos;

			public T Object {
				get { return m_Object; }
			}

			public Vector2 Position {
				get { return new Vector2(m_fXPos,m_fYPos); }
			}

			public float X {
				get { return m_fXPos; }
			}

			public float Y {
				get { return m_fYPos; }
			}
			
			public QuadtreeObject( T obj, Vector2 pos ) {
				m_Object = obj;
				m_fXPos = pos.X;
				m_fYPos = pos.Y;
			}

			public QuadtreeObject( T obj, float x, float y ) {
				m_Object = obj;
				m_fXPos = x;
				m_fYPos = y;
			}
		}

		// ----------------------------------------------------------------------------------------------------

		Quadtree<T> m_Parent;
		float m_fLeft, m_fRight, m_fTop, m_fBottom;
		int m_iMaxObjects, m_iDepth;
		OverCapacity m_funcOverCapacity;

		Quadtree<T>[] m_arrSubtrees;
		List<QuadtreeObject> m_arrObjects;

		// Subtrees in left to right order:
		// ,___ ___
		// |   |   |
		// | 0 | 1 |
		// |---+---|
		// | 2 | 3 |
		// |___|___|
		//
		// , = Zero point
		// ----------------------------------------------------------------------------------------------------

		public Quadtree<T> Parent {
			get { return m_Parent; }
			set { m_Parent = value; }
		}

		public float Left {
			get { return m_fLeft; }
			set { m_fLeft = (value < m_fRight ? value : m_fLeft); }
		}

		public float Right {
			get { return m_fRight; }
			set { m_fRight = (value > m_fLeft ? value : m_fRight); }
		}

		public float Top {
			get { return m_fTop; }
			set { m_fTop = (value < m_fBottom ? value : m_fTop); }
		}

		public float Bottom {
			get { return m_fBottom; }
			set { m_fBottom = (value > m_fTop ? value : m_fBottom); }
		}

		public float Width {
			get { return m_fRight - m_fLeft; }
		}

		public float Height {
			get { return m_fBottom - m_fTop; }
		}

		public int MaxObjects {
			get { return m_iMaxObjects; }
			set { m_iMaxObjects = (value > 1 ? value : 1); }
		}

		public int Depth {
			get { return m_iDepth; }
			set { m_iDepth = value; }
		}

		public OverCapacity OverCapacityFunc {
			get { return m_funcOverCapacity; }
			set { m_funcOverCapacity = value; }
		}
		
		public void SetPos( Vector2 v ) { SetPos(v.X,v.Y); }
		public void SetPos( float x, float y ) {
			float w = m_fRight - m_fLeft;
			float h = m_fBottom - m_fTop;

			m_fLeft = x;
			m_fTop = y;
			m_fRight = m_fLeft + w;
			m_fBottom = m_fTop + h;
		}

		public void SetSize( Vector2 size ) { SetPos(size.X,size.Y); }
		public void SetSize( float w, float h ) {
			m_fRight = m_fLeft + (w > 0 ? w : -w);
			m_fBottom = m_fTop + (h > 0 ? h : -h);
		}

		public void SetDimensions( Rectangle rect ) { SetDimensions(rect.X,rect.Y,rect.Width,rect.Height); }
		public void SetDimensions( Vector2 pos, Vector2 size ) { SetDimensions(pos.X,pos.Y,size.X,size.Y); }
		public void SetDimensions( float x, float y, float w, float h ) {
			m_fLeft = x;
			m_fTop = y;
			m_fRight = x + (w > 0 ? w : -w);
			m_fBottom = y + (h > 0 ? h : -h);
		}

		public Quadtree<T>[] Subtrees {
			get { return m_arrSubtrees; }
		}

		public List<QuadtreeObject> Objects {
			get { return m_arrObjects; }
		}

		public int ObjectCount {
			get { return m_arrObjects != null ? m_arrObjects.Count : 0; }
		}

		public void Clear() {
			// Safe or lazy clean?
		}

		// ----------------------------------------------------------------------------------------------------

		public Quadtree( Rectangle rect, int max ) {
			SetDimensions( rect );
			m_iMaxObjects = (max > 1 ? max : 1);
			m_iDepth = 0;
		}

		public Quadtree( Vector2 pos, Vector2 size, int max ) {
			SetDimensions( pos, size );
			m_iMaxObjects = (max > 1 ? max : 1);
			m_iDepth = 0;
		}

		public Quadtree( float x, float y, float w, float h, int max ) {
			SetDimensions( x, y, w, h );
			m_iMaxObjects = (max > 1 ? max : 1);
			m_iDepth = 0;
		}

		// ----------------------------------------------------------------------------------------------------

		public bool Add( T obj, Vector2 pos ) { return Add(new QuadtreeObject(obj,pos.X,pos.Y)); }
		public bool Add( T obj, float x, float y ) { return Add(new QuadtreeObject(obj,x,y)); }
		public bool Add( QuadtreeObject qtobj ) {
			// Inclusive - exclusive
			if( qtobj.X < m_fLeft || qtobj.X >= m_fRight || qtobj.Y < m_fTop || qtobj.Y >= m_fBottom )
				return false;

			if( m_arrSubtrees == null ) {
				if( m_arrObjects == null )
					m_arrObjects = new List<QuadtreeObject>();

				m_arrObjects.Add( qtobj );

				if( (m_funcOverCapacity == null && m_arrObjects.Count > m_iMaxObjects) || (m_funcOverCapacity != null && m_funcOverCapacity(this,qtobj,m_arrObjects,m_iMaxObjects)) ) {
					Subdivide();

					for(int i=0;i<m_arrObjects.Count;i++) {
						for(int t=0;t<4;t++) {
							if( m_arrSubtrees[t].Add(m_arrObjects[i]) ) {
								break;
							}
						}
					}

					m_arrObjects.Clear();
					m_arrObjects = null;
				}
			} else {
				for(int t=0;t<4;t++) {
					if( m_arrSubtrees[t].Add(qtobj) ) {
						break;
					}
				}
			}

			return true;
		}

		public void Add( T[] objects, Vector2[] positions ) {
			if( objects.Length != positions.Length ) {
				throw new ArgumentException( "Array sizes in arguments do not match!" );
			}

			for(int i=0;i<objects.Length;i++) {
				Add( objects[i], positions[i] );
			}
		}

		public void Add( QuadtreeObject[] qtobjects ) {
			for(int i=0;i<qtobjects.Length;i++) {
				Add( qtobjects );
			}
		}
		
		public Quadtree<T> FindTreeAt( Vector2 pos, ref List<Quadtree<T>> cascade ) { return FindTreeAt(pos.X,pos.Y,ref cascade); }
		public Quadtree<T> FindTreeAt( float x, float y, ref List<Quadtree<T>> cascade ) {
			// Inclusive - exclusive
			if( x < m_fLeft || x >= m_fRight || y < m_fTop || y >= m_fBottom )
				return null;

			if( cascade != null )
				cascade.Add( this );

			if( m_arrSubtrees != null ) {
				Quadtree<T> ret;
				for(int i=0;i<4;i++) {
					ret = m_arrSubtrees[i].FindTreeAt( x, y, ref cascade );
					if( ret != null ) {
						return ret;
					}
				}
			}

			return this;
		}

		public void Subdivide() {
			if( m_arrSubtrees != null )
				return;

			float hw = (m_fRight - m_fLeft) * 0.5f;
			float hh = (m_fBottom - m_fTop) * 0.5f;

			m_arrSubtrees = new Quadtree<T>[4];
			m_arrSubtrees[0] = new Quadtree<T>( m_fLeft,    m_fTop,    hw, hh, m_iMaxObjects );
			m_arrSubtrees[0].Parent = this;
			m_arrSubtrees[0].OverCapacityFunc = m_funcOverCapacity;
			m_arrSubtrees[0].Depth = m_iDepth + 1;

			m_arrSubtrees[1] = new Quadtree<T>( m_fLeft+hw, m_fTop,    hw, hh, m_iMaxObjects );
			m_arrSubtrees[1].Parent = this;
			m_arrSubtrees[1].OverCapacityFunc = m_funcOverCapacity;
			m_arrSubtrees[1].Depth = m_iDepth + 1;

			m_arrSubtrees[2] = new Quadtree<T>( m_fLeft,    m_fTop+hh, hw, hh, m_iMaxObjects );
			m_arrSubtrees[2].Parent = this;
			m_arrSubtrees[2].OverCapacityFunc = m_funcOverCapacity;
			m_arrSubtrees[2].Depth = m_iDepth + 1;

			m_arrSubtrees[3] = new Quadtree<T>( m_fLeft+hw, m_fTop+hh, hw, hh, m_iMaxObjects );
			m_arrSubtrees[3].Parent = this;
			m_arrSubtrees[3].OverCapacityFunc = m_funcOverCapacity;
			m_arrSubtrees[3].Depth = m_iDepth + 1;
		}

		public void CascadeSubdivide( int levels ) {
			if( levels <= 0 )
				return;

			Subdivide();
			m_arrSubtrees[0].CascadeSubdivide( levels-1 );
			m_arrSubtrees[1].CascadeSubdivide( levels-1 );
			m_arrSubtrees[2].CascadeSubdivide( levels-1 );
			m_arrSubtrees[3].CascadeSubdivide( levels-1 );
		}

		public List<Quadtree<T>> FindLeaves( float x, float y, float w, float h ) {
			if( x < m_fLeft && x+w >= m_fRight && y < m_fTop && y >= m_fBottom )
				return null;

			List<Quadtree<T>> leaves = new List<Quadtree<T>>();
			Stack<Quadtree<T>> stack = new Stack<Quadtree<T>>();
			stack.Push( this );
			while( stack.Count > 0 ) {
				Quadtree<T> top = stack.Pop();
				//if( x >= top.Left && x+w < top.Right && y >= top.Top && y+h < top.Bottom ) {
				if( !(x > top.Right || x+w < top.Left || y > top.Bottom || y+h < top.Top) ) {
					// In range
					if( top.Subtrees != null ) {
						// Not a leaf
						foreach( Quadtree<T> qt in top.Subtrees ) {
							stack.Push( qt );
						}
					} else if( top.ObjectCount > 0 ) {
						// Leaf
						leaves.Add( top );
					}
				}
			}

			return leaves;
		}

		// ----------------------------------------------------------------------------------------------------

		public void DebugDraw( SpriteBatch spriteBatch, Texture2D tex ) { DebugDraw(spriteBatch,tex,Color.Red); }
		public void DebugDraw( SpriteBatch spriteBatch, Texture2D tex, Color col ) {
			spriteBatch.Draw( tex, new Rectangle((int)m_fLeft,(int)m_fTop,(int)(m_fRight-m_fLeft),1), col );
			spriteBatch.Draw( tex, new Rectangle((int)m_fRight-1,(int)m_fTop,1,(int)(m_fBottom-m_fTop)), col );
			spriteBatch.Draw( tex, new Rectangle((int)m_fLeft,(int)m_fBottom-1,(int)(m_fRight-m_fLeft),1), col );
			spriteBatch.Draw( tex, new Rectangle((int)m_fLeft,(int)m_fTop,1,(int)(m_fBottom-m_fTop)), col );
		}

		public void DebugDrawAll( SpriteBatch spriteBatch, Texture2D tex ) { DebugDrawAll(spriteBatch,tex,Color.Red); }
		public void DebugDrawAll( SpriteBatch spriteBatch, Texture2D tex, Color col ) {
			spriteBatch.Draw( tex, new Rectangle((int)m_fLeft,(int)m_fTop,(int)(m_fRight-m_fLeft),1), col );
			spriteBatch.Draw( tex, new Rectangle((int)m_fRight-1,(int)m_fTop,1,(int)(m_fBottom-m_fTop)), col );
			spriteBatch.Draw( tex, new Rectangle((int)m_fLeft,(int)m_fBottom-1,(int)(m_fRight-m_fLeft),1), col );
			spriteBatch.Draw( tex, new Rectangle((int)m_fLeft,(int)m_fTop,1,(int)(m_fBottom-m_fTop)), col );

			if( m_arrSubtrees != null ) {
				m_arrSubtrees[0].DebugDrawAll( spriteBatch, tex, col );
				m_arrSubtrees[1].DebugDrawAll( spriteBatch, tex, col );
				m_arrSubtrees[2].DebugDrawAll( spriteBatch, tex, col );
				m_arrSubtrees[3].DebugDrawAll( spriteBatch, tex, col );
			}

			if( m_arrObjects != null ) {
				for(int i=0;i<m_arrObjects.Count;i++) {
					QuadtreeObject qtobj = m_arrObjects[i];
					spriteBatch.Draw( tex, new Rectangle((int)qtobj.X-1,(int)qtobj.Y-1,2,2), Color.Orange );
				}
			}
		}
	}
}
